#include <cmath>
#include <vector>
#include <algorithm>

// Constants and global parameters
struct SafetyParams {
    static const int NPRMU; // 
};

struct ControlParams {
    int IPIP; // Flag for parallel processing
};

struct MPFParams {
    int IHXMS;    // Mass matrix handle
    int IHBLAMB;  // Rotational mass matrix handle
};

// Function declarations for external routines
void TRCHKELE(int elemID, int numNodes, int nodeID, 
              const std::vector<double>& transMat, 
              const std::vector<double>& beamTransMat,
              const std::string& elemType);

void MPF_ADD(int numValues, const double* values, int startIdx, 
             int destIdx, int increment, int matrixHandle);

void MTXAB(const std::vector<double>& matA, const double* vecB, double* vecResult);

void CROSSV(const double* vecA, const double* vecB, double* vecResult);

// Main function to calculate mass matrix of 1D elements
void BASME(
    std::vector<int>& IZ,          // Element connectivity and properties
    const std::vector<double>& X,   // Node coordinates [3][numNodes]
    const std::vector<double>& CM,  // Material properties
    std::vector<double>& XMS,       // Mass matrix (diagonal)
    std::vector<double>& BLAMB,     // Rotational mass matrix [3][3][numNodes]
    double& TIM,                    // Total mass (accumulated)
    std::vector<double>& BICOS,     // Transformation matrix [3][3]
    double& AL,                     // Element length (output)
    int MTYP,                       // Material type
    int& IERR,                      // Error flag (output)
    double ROEL3,                   // Element density parameter
    double& RT,                     // Translational mass (output)
    std::vector<double>& VECFR,     // Frame vectors [16][numNodes]
    int ITPRF,                      // Profile type
    const std::vector<double>& BSCPRP, // Beam section properties [21]
    int IPTIW,                      // Muscle bar indicator
    const std::vector<double>& XMUS, // Muscle properties [15][numMuscles]
    const std::vector<double>& CMUS, // Muscle material properties [NPRMU][numMuscles]
    int ITYP1D,                     // 1D element type
    double& BEAMRT,                 // Average rotational mass (output)
    int IDBEA,                      // Beam element ID
    const std::vector<double>& TRSFMBE, // Beam transformation matrix
    const std::vector<double>& TRSDISP, // Nodal displacements
    const std::vector<double>& TRSFMAT, // Material transformation matrix
    const std::vector<int>& NODFRA   // Node frame data [5][numNodes]
) {
    // Get global parameters
    ControlParams& control = getControlParams();
    MPFParams& mpf = getMPFParams();
    
    // Initialize variables
    IERR = 0;
    double OROEL3I = 1.0 / (ROEL3 * ROEL3); // Inverse squared density parameter
    
    // Get node numbers from connectivity array
    int N1 = IZ[1]; // Node 1 (1-based index)
    int N2 = IZ[2]; // Node 2
    int N3 = IZ[3]; // Node 3 (optional)
    int N4 = IZ[4]; // Node 4 (optional)
    
    // Temporary variables
    double RTI, RTJ; // Nodal translational masses
    double RXI, RYI, RZI, RXJ, RYJ, RZJ; // Nodal rotational masses
    double RHO, AREA; // Material density and cross-section area
    std::vector<double> BIC(9, 0.0); // Local transformation matrix [3][3]
    std::vector<double> P(9, 0.0);   // Temporary vector for parallel assembly
    
    // Process based on element type
    switch (ITYP1D) {
        case 0: // Bar elements
        case 1: // Beam elements
        {
            // Calculate element length
            double XX = X[3*(N2-1)]   - X[3*(N1-1)];   // X2-X1
            double YY = X[3*(N2-1)+1] - X[3*(N1-1)+1]; // Y2-Y1
            double ZZ = X[3*(N2-1)+2] - X[3*(N1-1)+2]; // Z2-Z1
            AL = std::sqrt(XX*XX + YY*YY + ZZ*ZZ);
            
            if (AL == 0.0) {
                IERR = 1; // Zero-length element error
                AL = 1.0; // Set default length
            }
            
            // Calculate translational mass based on element type
            if (MTYP == 404) { // Spring-damper element
                RTI = 0.5 * CM[9] * OROEL3I;
                RTJ = RTI;
            } 
            else if (MTYP == 405) { // Special element type
                RTI = 0.5 * CM[6] * AL / (1.0 + CM[5]) * OROEL3I;
                RTJ = RTI;
            }
            else if (MTYP == 423 && ITPRF == 1) { // Tapered beam (MJ modification)
                RTI = BSCPRP[6] * OROEL3I;
                RTJ = BSCPRP[13] * OROEL3I;
            }
            else if (IPTIW != 0) { // Muscle bar element
                // Bar mass = muscle mass * bar length / muscle length
                RTI = 0.5 * CMUS[28][IPTIW-1] * AL * OROEL3I / XMUS[13][IPTIW-1];
                RTJ = RTI;
            }
            else { // Standard beam/bar element
                RHO = CM[0] * OROEL3I;
                AREA = CM[5];
                RTI = 0.5 * RHO * AL * AREA;
                RTJ = RTI;
            }
            
            RT = 0.5 * (RTI + RTJ); // Average translational mass
            TIM += RTI + RTJ;       // Accumulate total mass
            
            // Assemble translational mass into diagonal mass matrix
            if (control.IPIP == 0) { // Serial processing
                // Node 1 mass
                int NN = 6*(N1-1);
                XMS[NN]   += RTI; // X direction
                XMS[NN+1] += RTI; // Y direction
                XMS[NN+2] += RTI; // Z direction
                
                // Node 2 mass
                NN = 6*(N2-1);
                XMS[NN]   += RTJ;
                XMS[NN+1] += RTJ;
                XMS[NN+2] += RTJ;
            } 
            else { // Parallel processing
                P[0] = P[1] = P[2] = RTI;
                MPF_ADD(3, P.data(), 1, 6*(N1-1)+1, 1, mpf.IHXMS);
                
                P[0] = P[1] = P[2] = RTJ;
                MPF_ADD(3, P.data(), 1, 6*(N2-1)+1, 1, mpf.IHXMS);
            }
            
            if (ITYP1D == 0) break; // Bars don't need rotational mass
            
            // BEAM ELEMENTS ONLY - Compute rotational mass
            
            // Step 0: Check element transformation
            TRCHKELE(IDBEA, 2, N1, TRSFMAT, TRSFMBE, "BEAM");
            
            // Step 1: Compute local frame based on initial position
            double XXO = (X[3*(N2-1)]   - TRSDISP[3*(N2-1)]) - 
                         (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
            double YYO = (X[3*(N2-1)+1] - TRSDISP[3*(N2-1)+1]) - 
                         (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
            double ZZO = (X[3*(N2-1)+2] - TRSDISP[3*(N2-1)+2]) - 
                         (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
            double ALO = std::sqrt(XXO*XXO + YYO*YYO + ZZO*ZZO);
            double ALOV = 1.0 / ALO;
            
            // Local x-axis (element axis)
            BIC[0] = XXO * ALOV; // BIC(1,1)
            BIC[3] = YYO * ALOV; // BIC(2,1)
            BIC[6] = ZZO * ALOV; // BIC(3,1)
            
            // Define local y-axis (need a reference vector)
            double VAX, VAY, VAZ;
            if (N3 == 0) {
                // No third node - create orthogonal vector
                double BX = std::abs(BIC[0]);
                double BY = std::abs(BIC[3]);
                double BZ = std::abs(BIC[6]);
                double BN = std::min({BX, BY, BZ});
                
                if (BX == BN) {
                    VAX = 1.0; VAY = 0.0; VAZ = 0.0;
                } 
                else if (BY == BN) {
                    VAX = 0.0; VAY = 1.0; VAZ = 0.0;
                } 
                else {
                    VAX = 0.0; VAY = 0.0; VAZ = 1.0;
                }
            } 
            else {
                // Use third node to define reference vector
                VAX = (X[3*(N3-1)]   - TRSDISP[3*(N3-1)]) - 
                      (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
                VAY = (X[3*(N3-1)+1] - TRSDISP[3*(N3-1)+1]) - 
                      (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
                VAZ = (X[3*(N3-1)+2] - TRSDISP[3*(N3-1)+2]) - 
                      (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
            }
            
            // Normalize reference vector
            double A = std::sqrt(VAX*VAX + VAY*VAY + VAZ*VAZ);
            if (A == 0.0) {
                IERR = 2; // Zero-length vector error
            } 
            else {
                double AX = VAX / A;
                double AY = VAY / A;
                double AZ = VAZ / A;
                
                // Local z-axis (cross product of x-axis and reference vector)
                double E3X = AZ*BIC[3] - AY*BIC[6];
                double E3Y = AX*BIC[6] - AZ*BIC[0];
                double E3Z = AY*BIC[0] - AX*BIC[3];
                double ANORM = std::sqrt(E3X*E3X + E3Y*E3Y + E3Z*E3Z);
                
                if (ANORM == 0.0) {
                    IERR = 3; // Parallel vectors error
                } 
                else {
                    // Normalize z-axis
                    BIC[2] = E3X / ANORM; // BIC(1,3)
                    BIC[5] = E3Y / ANORM; // BIC(2,3)
                    BIC[8] = E3Z / ANORM; // BIC(3,3)
                    
                    // Local y-axis (cross product of z and x axes)
                    CROSSV(&BIC[2], &BIC[0], &BIC[1]);
                    
                    // Step 2: Apply transformation to local frame
                    MTXAB(TRSFMBE, &BIC[0], &BICOS[0]);
                    MTXAB(TRSFMBE, &BIC[1], &BICOS[3]);
                    CROSSV(&BICOS[0], &BICOS[3], &BICOS[6]);
                    CROSSV(&BICOS[6], &BICOS[0], &BICOS[3]);
                }
            }
            
            // Calculate rotational mass terms
            if (MTYP == 213 && ITPRF == 1) { // Tapered beam (MJ modification)
                RXI = BSCPRP[7] * OROEL3I;
                RYI = BSCPRP[8] * OROEL3I;
                RZI = BSCPRP[9] * OROEL3I;
                RXJ = BSCPRP[14] * OROEL3I;
                RYJ = BSCPRP[15] * OROEL3I;
                RZJ = BSCPRP[16] * OROEL3I;
            } 
            else { // Standard beam
                RHO = CM[0] * OROEL3I;
                AREA = CM[5];
                double RL = 0.5 * RHO * AL;
                double RR = AREA * AL * AL / 12.0;
                RXI = CM[7] * RL;  // Ixx
                RYI = (CM[8] + RR) * RL; // Iyy + contribution from area
                RZI = (CM[9] + RR) * RL; // Izz + contribution from area
                RXJ = RXI;
                RYJ = RYI;
                RZJ = RZI;
            }
            
            BEAMRT = (RXI + RYI + RZI + RXJ + RYJ + RZJ) / 6.0;
            
            // Assemble rotational mass matrix
            for (int II = 0; II < 3; II++) {
                for (int JJ = 0; JJ < 3; JJ++) {
                    // Node 1 rotational mass
                    double BADD = BICOS[II]   * BICOS[JJ]   * RXI + 
                                 BICOS[II+3] * BICOS[JJ+3] * RYI + 
                                 BICOS[II+6] * BICOS[JJ+6] * RZI;
                    
                    if (control.IPIP == 0) {
                        BLAMB[9*(N1-1) + 3*JJ + II] += BADD;
                    } 
                    else {
                        int NN = II + 3*JJ + 9*(N1-1);
                        MPF_ADD(1, &BADD, 1, NN+1, 1, mpf.IHBLAMB);
                    }
                    
                    // Node 2 rotational mass
                    BADD = BICOS[II]   * BICOS[JJ]   * RXI + 
                           BICOS[II+3] * BICOS[JJ+3] * RYI + 
                           BICOS[II+6] * BICOS[JJ+6] * RZI;
                    
                    if (control.IPIP == 0) {
                        BLAMB[9*(N2-1) + 3*JJ + II] += BADD;
                    } 
                    else {
                        NN = II + 3*JJ + 9*(N2-1);
                        MPF_ADD(1, &BADD, 1, NN+1, 1, mpf.IHBLAMB);
                    }
                }
            }
            
            // Add eccentricity and product of inertia terms if specified
            if (CM[125] != 0.0 || CM[126] != 0.0 || CM[127] != 0.0) {
                double RYZI = 0.5 * RHO * AL * CM[125];
                double RXYI = -0.25 * RHO * AL * AL * CM[127];
                double RXZI = -0.25 * RHO * AL * AL * CM[126];
                double RYZJ = RYZI;
                double RXYJ = RXYI;
                double RXZJ = RXZI;
                
                if (MTYP == 213 && ITPRF == 1) { // Tapered beam
                    RXYI = BSCPRP[10] * OROEL3I;
                    RYZI = BSCPRP[11] * OROEL3I;
                    RXZI = BSCPRP[12] * OROEL3I;
                    RXYJ = BSCPRP[17] * OROEL3I;
                    RYZJ = BSCPRP[18] * OROEL3I;
                    RXZJ = BSCPRP[19] * OROEL3I;
                }
                
                // Add product of inertia terms to rotational mass matrix
                for (int II = 0; II < 3; II++) {
                    for (int JJ = 0; JJ < 3; JJ++) {
                        // Node 1 terms
                        double BADD = -BICOS[II+3] * BICOS[JJ+6] * RYZI 
                                      - BICOS[II+6] * BICOS[JJ+3] * RYZI
                                      - BICOS[II]   * BICOS[JJ+3] * RXYI
                                      - BICOS[II+3] * BICOS[JJ]   * RXYI
                                      - BICOS[II]   * BICOS[JJ+6] * RXZI
                                      - BICOS[II+6] * BICOS[JJ]   * RXZI;
                        
                        if (control.IPIP == 0) {
                            BLAMB[9*(N1-1) + 3*JJ + II] += BADD;
                        } 
                        else {
                            int NN = II + 3*JJ + 9*(N1-1);
                            MPF_ADD(1, &BADD, 1, NN+1, 1, mpf.IHBLAMB);
                        }
                        
                        // Node 2 terms
                        BADD = -BICOS[II+3] * BICOS[JJ+6] * RYZJ 
                               - BICOS[II+6] * BICOS[JJ+3] * RYZJ
                               - BICOS[II]   * BICOS[JJ+3] * RXYJ
                               - BICOS[II+3] * BICOS[JJ]   * RXYJ
                               - BICOS[II]   * BICOS[JJ+6] * RXZJ
                               - BICOS[II+6] * BICOS[JJ]   * RXZJ;
                        
                        if (control.IPIP == 0) {
                            BLAMB[9*(N2-1) + 3*JJ + II] += BADD;
                        } 
                        else {
                            NN = II + 3*JJ + 9*(N2-1);
                            MPF_ADD(1, &BADD, 1, NN+1, 1, mpf.IHBLAMB);
                        }
                    }
                }
            }
            break;
        }
        
        case 2: // Spring elements
        case 3: // Joint elements
        {
            double PMASS = 0.0;
            
            if (MTYP == 420) { // Spring element
                PMASS = std::max({CM[4], CM[13], CM[22]}) * OROEL3I;
                
                if (control.IPIP == 0) { // Serial processing
                    // Node 1 mass
                    int NN = 6*(N1-1);
                    XMS[NN]   += 0.5 * PMASS;
                    XMS[NN+1] += 0.5 * PMASS;
                    XMS[NN+2] += 0.5 * PMASS;
                    
                    // Node 2 mass
                    NN = 6*(N2-1);
                    XMS[NN]   += 0.5 * PMASS;
                    XMS[NN+1] += 0.5 * PMASS;
                    XMS[NN+2] += 0.5 * PMASS;
                } 
                else { // Parallel processing
                    P[0] = P[1] = P[2] = 0.5 * PMASS;
                    MPF_ADD(3, P.data(), 1, 6*(N1-1)+1, 1, mpf.IHXMS);
                    MPF_ADD(3, P.data(), 1, 6*(N2-1)+1, 1, mpf.IHXMS);
                }
            }
            
            RT = 0.5 * PMASS;
            TIM += PMASS;
            
            // Check element transformation
            TRCHKELE(IDBEA, 2, N1, TRSFMAT, TRSFMBE, "SPRING/JOINT");
            
            if (N3 != N4) { // Frame defined by nodes N3 and N4
                // Compute local frame based on initial configuration
                double XX = (X[3*(N3-1)]   - TRSDISP[3*(N3-1)]) - 
                            (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
                double YY = (X[3*(N3-1)+1] - TRSDISP[3*(N3-1)+1]) - 
                            (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
                double ZZ = (X[3*(N3-1)+2] - TRSDISP[3*(N3-1)+2]) - 
                            (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
                AL = std::sqrt(XX*XX + YY*YY + ZZ*ZZ);
                
                if (AL == 0.0) {
                    IERR = 2;
                } 
                else {
                    // Local x-axis
                    BIC[0] = XX / AL;
                    BIC[3] = YY / AL;
                    BIC[6] = ZZ / AL;
                    
                    // Reference vector from N1 to N4
                    double VAX = (X[3*(N4-1)]   - TRSDISP[3*(N4-1)]) - 
                                 (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
                    double VAY = (X[3*(N4-1)+1] - TRSDISP[3*(N4-1)+1]) - 
                                 (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
                    double VAZ = (X[3*(N4-1)+2] - TRSDISP[3*(N4-1)+2]) - 
                                 (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
                    
                    double A = std::sqrt(VAX*VAX + VAY*VAY + VAZ*VAZ);
                    if (A == 0.0) {
                        IERR = 2;
                    } 
                    else {
                        double AX = VAX / A;
                        double AY = VAY / A;
                        double AZ = VAZ / A;
                        
                        // Local z-axis (cross product)
                        double E3X = AZ*BIC[3] - AY*BIC[6];
                        double E3Y = AX*BIC[6] - AZ*BIC[0];
                        double E3Z = AY*BIC[0] - AX*BIC[3];
                        double ANORM = std::sqrt(E3X*E3X + E3Y*E3Y + E3Z*E3Z);
                        
                        if (ANORM == 0.0) {
                            IERR = 3;
                        } 
                        else {
                            // Normalize z-axis
                            BIC[2] = E3X / ANORM;
                            BIC[5] = E3Y / ANORM;
                            BIC[8] = E3Z / ANORM;
                            
                            // Local y-axis (cross product)
                            CROSSV(&BIC[2], &BIC[0], &BIC[1]);
                            
                            // Apply transformation to local frame
                            MTXAB(TRSFMBE, &BIC[0], &BICOS[0]);
                            MTXAB(TRSFMBE, &BIC[1], &BICOS[3]);
                            CROSSV(&BICOS[0], &BICOS[3], &BICOS[6]);
                            CROSSV(&BICOS[6], &BICOS[0], &BICOS[3]);
                        }
                    }
                }
            } 
            else { // Frame defined by frame number
                int ITYPFR = NODFRA[5*(N3-1)]; // Frame type
                if (ITYPFR == 0) { // Frame attached to spring node
                    MTXAB(TRSFMBE, &VECFR[16*(N3-1)], &BICOS[0]);
                    MTXAB(TRSFMBE, &VECFR[16*(N3-1)+3], &BICOS[3]);
                    CROSSV(&BICOS[0], &BICOS[3], &BICOS[6]);
                    CROSSV(&BICOS[6], &BICOS[0], &BICOS[3]);
                } 
                else { // Fixed frame
                    for (int II = 0; II < 3; II++) {
                        BICOS[II]   = VECFR[16*(N3-1) + II];
                        BICOS[II+3] = VECFR[16*(N3-1) + 3 + II];
                        BICOS[II+6] = VECFR[16*(N3-1) + 6 + II];
                    }
                }
            }
            
            // Assemble rotational mass for spring elements
            if (MTYP == 420) {
                double RX = 0.5 * CM[31] * OROEL3I;
                double RY = 0.5 * CM[40] * OROEL3I;
                double RZ = 0.5 * CM[49] * OROEL3I;
                BEAMRT = (RX + RY + RZ) / 3.0;
                
                for (int M = 1; M <= 2; M++) { // Nodes 2 and 3
                    int N = IZ[M+1]; // N2 or N3
                    for (int II = 0; II < 3; II++) {
                        for (int JJ = 0; JJ < 3; JJ++) {
                            double BADD = BICOS[II]   * BICOS[JJ]   * RX + 
                                         BICOS[II+3] * BICOS[JJ+3] * RY + 
                                         BICOS[II+6] * BICOS[JJ+6] * RZ;
                            
                            if (control.IPIP == 0) {
                                BLAMB[9*(N-1) + 3*JJ + II] += BADD;
                            } 
                            else {
                                int NN = II + 3*JJ + 9*(N-1);
                                MPF_ADD(1, &BADD, 1, NN+1, 1, mpf.IHBLAMB);
                            }
                        }
                    }
                }
            }
            break;
        }
        
        case 8: // Spring-beam elements
        {
            double PMASS = std::max({CM[4], CM[13], CM[22]}) * OROEL3I;
            
            // Assemble translational mass
            if (control.IPIP == 0) {
                // Node 1 mass
                int NN = 6*(N1-1);
                XMS[NN]   += 0.5 * PMASS;
                XMS[NN+1] += 0.5 * PMASS;
                XMS[NN+2] += 0.5 * PMASS;
                
                // Node 2 mass
                NN = 6*(N2-1);
                XMS[NN]   += 0.5 * PMASS;
                XMS[NN+1] += 0.5 * PMASS;
                XMS[NN+2] += 0.5 * PMASS;
            } 
            else {
                P[0] = P[1] = P[2] = 0.5 * PMASS;
                MPF_ADD(3, P.data(), 1, 6*(N1-1)+1, 1, mpf.IHXMS);
                MPF_ADD(3, P.data(), 1, 6*(N2-1)+1, 1, mpf.IHXMS);
            }
            
            RT = 0.5 * PMASS;
            TIM += PMASS;
            
            // Calculate element length
            double XX = X[3*(N2-1)]   - X[3*(N1-1)];
            double YY = X[3*(N2-1)+1] - X[3*(N1-1)+1];
            double ZZ = X[3*(N2-1)+2] - X[3*(N1-1)+2];
            AL = std::sqrt(XX*XX + YY*YY + ZZ*ZZ);
            
            if (AL == 0.0) {
                IERR = 1;
                AL = 1.0;
            }
            
            // Check element transformation
            TRCHKELE(IDBEA, 2, N1, TRSFMAT, TRSFMBE, "SPRING-BEAM");
            
            // Compute local frame based on initial position
            double XXO = (X[3*(N2-1)]   - TRSDISP[3*(N2-1)]) - 
                         (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
            double YYO = (X[3*(N2-1)+1] - TRSDISP[3*(N2-1)+1]) - 
                         (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
            double ZZO = (X[3*(N2-1)+2] - TRSDISP[3*(N2-1)+2]) - 
                         (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
            double ALO = std::sqrt(XXO*XXO + YYO*YYO + ZZO*ZZO);
            double ALOV = 1.0 / ALO;
            
            // Local x-axis
            BIC[0] = XXO * ALOV;
            BIC[3] = YYO * ALOV;
            BIC[6] = ZZO * ALOV;
            
            if (N3 != 0 || (N3 == 0 && N4 == 0)) { // Frame defined by N1,N2,N3 or symmetric
                double VAX, VAY, VAZ;
                if (N3 == 0) { // Create orthogonal vector
                    double BX = std::abs(BIC[0]);
                    double BY = std::abs(BIC[3]);
                    double BZ = std::abs(BIC[6]);
                    double BN = std::min({BX, BY, BZ});
                    
                    if (BX == BN) {
                        VAX = 1.0; VAY = 0.0; VAZ = 0.0;
                    } 
                    else if (BY == BN) {
                        VAX = 0.0; VAY = 1.0; VAZ = 0.0;
                    } 
                    else {
                        VAX = 0.0; VAY = 0.0; VAZ = 1.0;
                    }
                } 
                else { // Use N3 to define reference vector
                    VAX = (X[3*(N3-1)]   - TRSDISP[3*(N3-1)]) - 
                          (X[3*(N1-1)]   - TRSDISP[3*(N1-1)]);
                    VAY = (X[3*(N3-1)+1] - TRSDISP[3*(N3-1)+1]) - 
                          (X[3*(N1-1)+1] - TRSDISP[3*(N1-1)+1]);
                    VAZ = (X[3*(N3-1)+2] - TRSDISP[3*(N3-1)+2]) - 
                          (X[3*(N1-1)+2] - TRSDISP[3*(N1-1)+2]);
                }
                
                double A = std::sqrt(VAX*VAX + VAY*VAY + VAZ*VAZ);
                if (A == 0.0) {
                    IERR = 2;
                } 
                else {
                    double AX = VAX / A;
                    double AY = VAY / A;
                    double AZ = VAZ / A;
                    
                    // Local z-axis
                    double E3X = AZ*BIC[3] - AY*BIC[6];
                    double E3Y = AX*BIC[6] - AZ*BIC[0];
                    double E3Z = AY*BIC[0] - AX*BIC[3];
                    double ANORM = std::sqrt(E3X*E3X + E3Y*E3Y + E3Z*E3Z);
                    
                    if (ANORM == 0.0) {
                        IERR = 3;
                    } 
                    else {
                        // Normalize z-axis
                        BIC[2] = E3X / ANORM;
                        BIC[5] = E3Y / ANORM;
                        BIC[8] = E3Z / ANORM;
                        
                        // Local y-axis
                        CROSSV(&BIC[2], &BIC[0], &BIC[1]);
                        
                        // Apply transformation
                        MTXAB(TRSFMBE, &BIC[0], &BICOS[0]);
                        MTXAB(TRSFMBE, &BIC[1], &BICOS[3]);
                        CROSSV(&BICOS[0], &BICOS[3], &BICOS[6]);
                        CROSSV(&BICOS[6], &BICOS[0], &BICOS[3]);
                    }
                }
            } 
            else if (N4 != 0) { // Frame defined by frame number N4
                MTXAB(TRSFMBE, &BIC[0], &BICOS[0]);
                MTXAB(TRSFMBE, &VECFR[16*(N4-1)+3], &BICOS[3]);
                
                // Check orthogonality
                double C1 = BICOS[1]*BICOS[8] - BICOS[2]*BICOS[